//5_30 	测试hwt101驱动程序
//6_14	速度叠加算法验证
//6_24	电机驱动编写
//6_27	麦轮运动学算法编写完成
//6_29	数码管驱动函数
//6_26	云台调试动作
//7_11	添加串口数据包
//7_18	视觉调试颜色
//7_24	添加软复位

#include "main.h"

char test_data[6] = {'1', '2', '3', '3', '2', '1'};

static char* qr_data = NULL;						//扫码数据包
static int* move = NULL;							//半成品/成品区动作
static u8 *color_data = NULL;						//颜色数据

//任务句柄
TaskHandle_t start_task_handler;
TaskHandle_t tast_loop_handler;
TaskHandle_t circle_location_task_handler;
TaskHandle_t line_location_task_handler;
TaskHandle_t volatge_check_task_handler;
TaskHandle_t second_task_loop_handler;

QueueHandle_t speed_x_queue;
QueueHandle_t speed_y_queue;
QueueHandle_t speed_w_queue;

int main(void)
{
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);			//设置系统中断优先级分组4
	delay_init(168);										//延时初始化
	usart1_init(9600);										//调试端口、树莓派通信端口
	scan_qr_code_Init();									//扫码初始化
	digital_tube_Init();									//数码管模块初始化
	usart_ctrl_steeping_motoe_Init();						//串口控制步进电机初始化
	servo_Init(20000 - 1, 84 - 1);							//舵机控制初始化
	motion_Init();											//动作软件数据初始化
	package_Init();											//串口数据包软件初始化
	motor_soft_Init();										//电机软件初始化

	while(motor_volatge < 10000)
	{
		read_motor_voltage();
		delay_xms(500);
	}

	tast_loop();
}

//主循环
void tast_loop(void)
{
	delay_xms(5000);	//等待电机启动

	steeping_motor motor1;
	motor1.motor_ID = 1;
	
	//set_gimbal_motor_zero(motor1);	//设置电机归零
	gimbal_motor_return_zero(motor1);	//云台电机归位         
	delay_xms(2000);

	{	//到物料盘
	change_xyw_speed_no_quene(0,-100,0);
	delay_xms(300);

	change_xyw_speed_no_quene(150,0,0);
	delay_xms(1200);

	change_xyw_speed_no_quene(0,0,0);
	delay_xms(500);
	
	//获取二维码
	while(qr_data == NULL) qr_data = get_qr_code_data();
	digital_tube_Displat(qr_data);
	//qr_data = test_data;
	delay_xms(200);
	
	change_xyw_speed_no_quene(153,28,0);
	delay_xms(1200);

	change_xyw_speed_no_quene(0,0,0);
	delay_xms(100);
	}

	//goto second;
	//颜色识别+抓取物料
#ifndef DEBUG_LOCATION
	for(char i = 1;i <= 3; i++)
	{
		//颜色判断
		for(int k = 0;k < 10;k ++)//清理pi缓存
		{
			call_pi(qr_data[i-1]);
			delay_xms(50);
		}

		reset_color_packge();

		while(1)
		{
			call_pi(qr_data[i-1]);

			color_data = get_color_data();
			if(color_data[0] == '1')
			{
				color_data = NULL;
				break;
			}
			delay_xms(100);
		}
		//夹取物料到车上
		delay_xms(1500);
		get_goods_on_turn_table();
		put_goods_on_plate(i);
		delay_xms(1500);

	}
#endif
	
	{//物料盘到暂存区
	//半成品区域根据二维码数字决定到哪个位置
	if(qr_data[0] == '1')
	{
		change_xyw_speed_no_quene(150,0,-88);
		delay_xms(1200);
		change_xyw_speed_no_quene(120, 15, 0);
	}
	else if(qr_data[0] == '2') 
	{
		change_xyw_speed_no_quene(150,0,-88);
		delay_xms(1200);
		change_xyw_speed_no_quene(155, 15, 0);
	}
	else if(qr_data[0] == '3') 
	{
		change_xyw_speed_no_quene(150,0,-88);
		delay_xms(1200);
		change_xyw_speed_no_quene(195, 15, 2);
	}

	delay_xms(800);
	change_xyw_speed_no_quene(0,0,0);
	delay_xms(100);
	}

	//半成品区放置三个物料
	for(char i = 0; i < 3; i++)
	{
		move = get_way(qr_data[i]);		//获取下一个目标
		change_xyw_speed_no_quene(move[1], 0, 0);
		delay_xms(move[0]);

		change_xyw_speed_no_quene(0,0,0);
		delay_xms(100);

		gimbal_motor_down(ground);
		for(int i = 0;i < 30;i ++)//清理pi缓存
		{
			call_pi(circle_center_identify);
			delay_xms(50);
		}

		circle_location_task(0);//圆心定位

		gimbal_motor_up(ground);
		delay_xms(500);

#ifndef DEBUG_LOCATION
		get_goods_on_plate(i + 1);
		put_goods_on_ground();
#endif
	}

	//半成品区捡起三个物料
	for(char i = 0; i < 3; i++)
	{
		move = get_way(qr_data[i]);		//获取下一个目标
		change_xyw_speed_no_quene(move[1],0,0);
		delay_xms(move[0]);

		change_xyw_speed_no_quene(0,0,0);
		delay_xms(100);

#ifndef DEBUG_LOCATION
		get_goods_on_ground();
		put_goods_on_plate(i + 1);
#endif
		delay_xms(500);
	}

	{//前往成品区域
	if(qr_data[2] == '1')//红
	{
		change_xyw_speed_no_quene(180,0,0);
		delay_xms(700);
		change_xyw_speed_no_quene(150,0,-78);
		delay_xms(1000);
		change_xyw_speed_no_quene(100, 5, 0);
		delay_xms(800);
	}
	else if(qr_data[2] == '2')//绿
	{
		change_xyw_speed_no_quene(180,0,0);
		delay_xms(500);
		change_xyw_speed_no_quene(150,0,-78);
		delay_xms(1000);
		change_xyw_speed_no_quene(100, 5, 0);
		delay_xms(800);
	}
	else if(qr_data[2] == '3')//蓝
	{
		change_xyw_speed_no_quene(170,0,-92);
		delay_xms(1200);
		change_xyw_speed_no_quene(165, 20, 0);
		delay_xms(400);
	}

	change_xyw_speed_no_quene(0, 0, 0);
	delay_xms(400);

	if(qr_data[0] == '2') 
	{
		change_xyw_speed_no_quene(130, 15, 0);
		delay_xms(320);
	}
	else if(qr_data[0] == '3')
	{
		change_xyw_speed_no_quene(130, 0, 0);
		delay_xms(550);
	}

	change_xyw_speed_no_quene(0, 0, 0);
	delay_xms(800);
	}

	//成品区放置三个物料
	for(char i = 0; i < 3; i++)
	{
		move = get_way(qr_data[i]);		//获取下一个目标
		change_xyw_speed_no_quene(move[1], 0, 0);
		delay_xms(move[0]);

		change_xyw_speed_no_quene(0,0,0);
		delay_xms(100);

		gimbal_motor_down(ground);
		for(int i = 0;i < 30;i ++)//清理pi缓存
		{
			call_pi(circle_center_identify);
			delay_xms(50);
		}

		circle_location_task(0);//圆心定位

		gimbal_motor_up(ground);
		delay_xms(500);

#ifndef DEBUG_LOCATION
		get_goods_on_plate(i + 1);
		put_goods_on_ground();
#endif
	}

	//需要刷新三次计数!!!!!!!!!
	for(char i = 0;i < 3; i++) get_way(qr_data[i]);

	{//第一程成品区返程,返回转盘
	if(qr_data[2] == '2')
	{
		change_xyw_speed_no_quene(-140, -15, 0);
		delay_xms(320);
	}
	else if(qr_data[2] == '3')
	{
		change_xyw_speed_no_quene(-130, -15, 0);
		delay_xms(550);
	}

	change_xyw_speed_no_quene(0, 0, 0);
	delay_xms(400);
	change_xyw_speed_no_quene(-190 ,0, 74);
	delay_xms(1500);
	change_xyw_speed_no_quene(-185,0,0);
	delay_xms(300);
	change_xyw_speed_no_quene(0, 0, 0);
	delay_xms(400);

	//到半成品区域
	gimbal_motor_down(ground);
	for(int i = 0;i < 30;i ++)//清理pi缓存
	{
		call_pi(circle_center_identify);
		delay_xms(50);
	}

	circle_location_task(0);//圆心定位

	delay_xms(500);
	change_xyw_speed_no_quene(0,-100,0);
	delay_xms(200);
	change_xyw_speed_no_quene(0, 0, 0);
	delay_xms(100);

	for(int i = 0;i < 30;i ++)//清理pi缓存
	{
		call_pi(line_degree_location);
		delay_xms(50);
	}

	line_location_task();//根据直线矫正角度

	gimbal_motor_up(ground);

	change_xyw_speed_no_quene(-180, 0, 0);
	delay_xms(500);
	change_xyw_speed_no_quene(-140,0,55);
	delay_xms(1200);
	change_xyw_speed_no_quene(0,0,0);
	delay_xms(100);
	}

//----------------------第二程开启----------------------------
	//second:
	//颜色识别+抓取物料
#ifndef DEBUG_LOCATION
	for(char i = 1;i <= 3; i++)
	{
		//颜色判断
		for(int k = 0;k < 10;k ++)//清理pi缓存
		{
			call_pi(qr_data[i-1+3]);
			delay_xms(50);
		}

		reset_color_packge();

		while(1)
		{
			call_pi(qr_data[i-1+3]);

			color_data = get_color_data();
			if(color_data[0] == '1')
			{
				color_data = NULL;
				break;
			}
			delay_xms(100);
		}
		//夹取物料到车上
		delay_xms(1500);
		get_goods_on_turn_table();
		put_goods_on_plate(i);
		
	}
#endif
	delay_xms(1500);

	{//物料盘到暂存区
	//半成品区域根据二维码数字决定到哪个位置
	if(qr_data[3] == '1') //pass
	{
		change_xyw_speed_no_quene(150,0,-88);
		delay_xms(1200);
		change_xyw_speed_no_quene(120, 15, 0);
	}
	else if(qr_data[3] == '2') 
	{
		change_xyw_speed_no_quene(150,0,-88);
		delay_xms(1200);
		change_xyw_speed_no_quene(155, 15, 0);
	}
	else if(qr_data[3] == '3') 
	{
		change_xyw_speed_no_quene(150,0,-88);
		delay_xms(1200);
		change_xyw_speed_no_quene(195, 15, 2);
	}

	delay_xms(800);
	change_xyw_speed_no_quene(0,0,0);
	delay_xms(100);
	}

	//半成品区放置三个物料
	for(char i = 0; i < 3; i++)
	{
		move = get_way(qr_data[i + 3]);		//获取下一个目标
		change_xyw_speed_no_quene(move[1], 0, 0);
		delay_xms(move[0]);

		change_xyw_speed_no_quene(0,0,0);
		delay_xms(100);

		gimbal_motor_down(ground);
		for(int i = 0;i < 30;i ++)//清理pi缓存
		{
			call_pi(circle_center_identify);
			delay_xms(50);
		}

		circle_location_task(0);//圆心定位

		gimbal_motor_up(ground);
		delay_xms(500);

#ifndef DEBUG_LOCATION
		get_goods_on_plate(i + 1);
		put_goods_on_ground();
#endif
	}

	//半成品区捡起三个物料
	for(char i = 0; i < 3; i++)
	{
		move = get_way(qr_data[i+3]);		//获取下一个目标
		change_xyw_speed_no_quene(move[1],0,0);
		delay_xms(move[0]);

		change_xyw_speed_no_quene(0,0,0);
		delay_xms(100);

#ifndef DEBUG_LOCATION
		get_goods_on_ground();
		put_goods_on_plate(i + 1);
#endif

		delay_xms(500);
	}

	{//前往成品区域
	if(qr_data[5] == '1')//红
	{
		change_xyw_speed_no_quene(180,0,0);
		delay_xms(700);
		change_xyw_speed_no_quene(150,0,-78);
		delay_xms(1000);
		change_xyw_speed_no_quene(100, 5, 0);
		delay_xms(800);
	}
	else if(qr_data[5] == '2')//绿
	{
		change_xyw_speed_no_quene(180,0,0);
		delay_xms(500);
		change_xyw_speed_no_quene(150,0,-78);
		delay_xms(1000);
		change_xyw_speed_no_quene(100, 5, 0);
		delay_xms(800);
	}
	else if(qr_data[5] == '3')//蓝
	{
		change_xyw_speed_no_quene(170,0,-92);
		delay_xms(1200);
		change_xyw_speed_no_quene(165, 20, 0);
		delay_xms(400);
	}

	change_xyw_speed_no_quene(0, 0, 0);
	delay_xms(400);

	if(qr_data[3] == '2') 
	{
		change_xyw_speed_no_quene(130, 15, 0);
		delay_xms(320);
	}
	else if(qr_data[3] == '3')
	{
		change_xyw_speed_no_quene(130, 0, 0);
		delay_xms(550);
	}

	change_xyw_speed_no_quene(0, 0, 0);
	delay_xms(800);
	}

	//成品区放置三个物料
	for(char i = 0; i < 3; i++)
	{
		move = get_way(qr_data[i+3]);		//获取下一个目标
		change_xyw_speed_no_quene(move[1], 0, 0);
		delay_xms(move[0]);

		change_xyw_speed_no_quene(0,0,0);
		delay_xms(100);

		for(int i = 0;i < 30;i ++)//清理pi缓存
		{
			call_pi(material_circle_location);
			delay_xms(50);
		}

		circle_location_task(qr_data[i+3] + 1 - 48);//圆心定位

#ifndef DEBUG_LOCATION
		get_goods_on_plate(i + 1);
		put_goods_on_second_floor();
#endif
		}		

	{//回家
	if(qr_data[5] == '2')
	{
		change_xyw_speed_no_quene(50,0,0);
		delay_xms(700);
		change_xyw_speed_no_quene(0,0,0);
		delay_xms(400);
	}
	else if(qr_data[5] == '1')
	{
		change_xyw_speed_no_quene(50,0,0);
		delay_xms(1400);
		change_xyw_speed_no_quene(0,0,0);
		delay_xms(400);
	}

	change_xyw_speed_no_quene(220,20,0);
	delay_xms(500);
	change_xyw_speed_no_quene(200,0,-78);
	delay_xms(1000);
	change_xyw_speed_no_quene(200, 15, 0);
	delay_xms(1000);
	change_xyw_speed_no_quene(200, 0, 0);
	delay_xms(400);
	change_xyw_speed_no_quene(0,0,0);
	delay_xms(400);
	}

	while(1) delay_xms(1000);
}

//监测电机电压，一旦电压低于10000mv，自动复位程序
void volatge_check_task()
{
	while(1)
	{
		read_motor_voltage();
		vTaskDelay(2000);
		if(motor_volatge == 666){}//bug：第一次读值为666，不知道为何
		else if(motor_volatge < 6000) 
		{
			printf("volatge:%dmv,restart\r\n", motor_volatge);
			vTaskDelay(1);
			NVIC_SystemReset();
		}
		else
		{
			printf("volatge:%dmv", motor_volatge);
		}
	}
}